#ifndef __TOOL_KIT__H
#define __TOOL_KIT__H
#include <rclcpp/rclcpp.hpp>

// 定义打印输出函数
#define DEBUG_OUT(info) RCLCPP_INFO_STREAM(rclcpp::get_logger("manual_handler"), info)
#define DEBUG_WARN_OUT(info) RCLCPP_WARN_STREAM(rclcpp::get_logger("manual_handler"), info)
#define DEBUG_ERROR_OUT(info) RCLCPP_ERROR_STREAM(rclcpp::get_logger("manual_handler"), info)
#define DEBUG_FATAL_OUT(info) RCLCPP_FATAL_STREAM(rclcpp::get_logger("manual_handler"), info)
#define DEBUG_OUT_THROTTLE(second,info) RCLCPP_INFO_STREAM_THROTTLE(rclcpp::get_logger("manual_handler"), std::chrono::seconds(second), info)

#define BIT(x) (((uint64_t)1) << (x))
#define TestBit(x, i) ((x & BIT(i)) != 0)
#define SET_BIT(x, n) (x | ((uint64_t)(1) << n))
#define CLEAR_BIT(x, n) (x & ~((uint64_t)(1) << n))
#define NEGATION_BIT(x, n) (x ^ ((uint64_t)(1) << n))
typedef unsigned long long TIME_TYPE;

//使用系统时钟时间
static inline TIME_TYPE GetTickCount()
{
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ((unsigned long long)(ts.tv_nsec / 1000000) + (unsigned long long)(ts.tv_sec * 1000ull));
}

//时间戳类
class CTimeStamp
{
public:
    TIME_TYPE m_timePoint;

public:
    CTimeStamp() { m_timePoint = GetTickCount(); }
    CTimeStamp(TIME_TYPE time) { m_timePoint = time; }

    TIME_TYPE GetTimeStamp() { return m_timePoint; }

    // Update the time-stamp
    void Update() { m_timePoint = GetTickCount(); }

    // Check whether the time-stamp is up-to-date
    bool IsNew(unsigned long timeDiff = 500)
    {
        return (fabs(GetTickCount() - m_timePoint) < timeDiff);
    }

    bool TimeOut(unsigned long timeDiff = 500)
    {
        return !IsNew(timeDiff);
    }
};

// C++11 thread safe
template <typename T>
class CDataTimeStamp : public CTimeStamp
{
public:
    T m_data;
    CDataTimeStamp()
    {
        m_data = T();
        CTimeStamp::Update();
    }

    CDataTimeStamp(T data)
    {
        Update(data);
    }

    void Update(T data)
    {
        m_data = data;
        CTimeStamp::Update();
    }

    void UpdateTime()
    {
        CTimeStamp::Update();
    }

    T Get()
    {
        return m_data;
    }
};

//
// 先stop 在start 判断是否触发一次(且切换一次只会触发一次，想要重复触发，需要再次stop->start)
//
//
struct CTimerTriggerOnce : public CTimeStamp
{
private:
    bool m_bRun;
    bool m_bTrigerOnce;
    std::mutex m_mutex;

public:
    CTimerTriggerOnce()
    {
        ClearTriger();
    }

    // 触发启动一次
    bool Start()
    {
        std::lock_guard<std::mutex> locker(m_mutex);
        if (!m_bRun)
        {
            m_bRun = true;
            m_bTrigerOnce = false;
            Update();
            DEBUG_WARN_OUT("Start Once");
            return true;
        }
        return false;
    }

    // 关闭运行
    bool Stop()
    {
        std::lock_guard<std::mutex> locker(m_mutex);
        if (m_bRun)
        {
            m_bRun = false;
            m_bTrigerOnce = false;
            DEBUG_WARN_OUT("Stop Once");
            return true;
        }
        Update();
        return false;
    }

    // 判断只是否触发一次
    bool TriggerOnce(uint16_t timeDiff = 3000)
    {
        std::lock_guard<std::mutex> locker(m_mutex);
        bool ret = false;
        if (m_bRun)
        {
            if (!m_bTrigerOnce)
            {
                // 触发一次
                ret = m_bTrigerOnce = TimeOut(timeDiff);
                DEBUG_WARN_OUT("Trigger m_bTrigerOnce=" << m_bTrigerOnce << ",timeDiff=" << timeDiff);
            }
        }

        return ret;
    }

    // 超时一直触发
    bool TriggerAlways(uint16_t timeDiff = 3000)
    {
        std::lock_guard<std::mutex> locker(m_mutex);
        bool ret = false;
        if (m_bRun)
        {
            ret = m_bTrigerOnce = TimeOut(timeDiff);
            DEBUG_WARN_OUT("Trigger m_bTrigerOnce=" << m_bTrigerOnce << ",timeDiff=" << timeDiff);
        }

        return ret;
    }

    void ClearTriger()
    {
        std::lock_guard<std::mutex> locker(m_mutex);
        m_bRun = false;
        m_bTrigerOnce = false;
        Update();
        DEBUG_WARN_OUT("ClearTriger");
    }
};

// std::stoi stol stoul stoll stof stod
inline bool string2Number(const std::string &str, long long &ret)
{
    bool ok = true;
    try
    {
        ret = stoll(str);
    }
    catch (...)
    {
        ok = false;
        DEBUG_OUT("error: string2longlong failed");
    }
    return ok;
}

inline bool string2Number(const std::string &str, int &ret)
{
    bool ok = true;
    try
    {
        ret = stoi(str);
    }
    catch (...)
    {
        ok = false;
        DEBUG_OUT("error: string2Int failed");
    }
    return ok;
}

inline bool string2Double(const std::string &str, double &ret)
{
    bool ok = true;
    try
    {
        ret = stod(str);
    }
    catch (...)
    {
        // everything else
        ok = false;
        DEBUG_OUT("error: string2Double failed");
    }
    return ok;
}

#endif